from tools.data_proc_func import transformAxis
import cv2
import numpy as np 

class lidar_painter:
    '''
        封装的雷达可视化类
    '''
    def __init__(self,max_range,side):
        '''
            - params
                - max_range:半径代表的物理距离
                - side:边长像素值
        '''
        self.side = side
        self.half_side = int(side/2)
        self.max_range = max_range
        self.img = self.drawPolarCoordinates(self.side,self.half_side)

    def drawPolarCoordinates(self,side,half_side):
        '''
            画出背景
            - params
                - side:边长像素值
                - half_side:半径,也是边长像素值的一半
            - return
                - img 背景图片,为一个numpy矩阵
        '''
        img = 255 - np.zeros([side,side,3],np.uint8)
        cv2.line(img,(0,half_side),(side,half_side),(0,0,0),1,cv2.LINE_AA)   
        cv2.line(img,(half_side,0),(half_side,side),(0,0,0),1,cv2.LINE_AA)
        cv2.line(img,(0,0),(side,side),(0,0,0),1,cv2.LINE_AA)
        cv2.line(img,(side,0),(0,side),(0,0,0),1,cv2.LINE_AA)
        min_radius = int(self.half_side/10)
        radius_list = [min_radius*(i+1) for i in range(15)]
        for radius in radius_list:
            cv2.circle(img,(self.half_side,self.half_side),radius,(0,0,0),thickness=1,lineType=cv2.LINE_AA)
        return img

    def drawLidarFrame(self,src_data):
        '''
            雷达数据可视化
            - params
                - src_data:原始的(角度,距离)元组列表
        '''
        img = self.img.copy()
        data = transformAxis(src_data,self.half_side,self.max_range)
        for elem in data:
            cv2.circle(img,(elem[0],elem[1]),1,(0,0,255),thickness=1,lineType=cv2.LINE_AA)
        cv2.imshow('rplidar -radius {}'.format(self.max_range),img)
        if cv2.waitKey(1) & 0xFF == 27:
            return True
        else:
            return False


if __name__ == '__main__':
    pass